#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Vector3Stamped
from std_msgs.msg import Float64, Header
import numpy as np

class MockGimbal:
    def __init__(self):
        rospy.init_node('mock_gimbal_node', anonymous=True)
        rospy.loginfo("Mock Gimbal Node Initializing...")

        self.rate = rospy.Rate(50) # Faster than control loop for smoother simulation

        self.current_pitch_deg = 0.0
        self.current_roll_deg = 0.0

        # --- Publishers ---
        self.pitch_feedback_pub = rospy.Publisher('/gimbal_controller/pitch_angle_feedback', Float64, queue_size=1)

        # --- Subscribers ---
        rospy.Subscriber('/gimbal_controller/command/euler', Vector3Stamped, self._gimbal_command_cb)

        rospy.loginfo("Mock Gimbal Node Ready.")

    def _gimbal_command_cb(self, msg: Vector3Stamped):
        # In a real gimbal, this would command a motor, and feedback would come from encoder
        # Here, we just assume it immediately reaches the commanded angle for simplicity
        self.current_pitch_deg = msg.vector.x
        self.current_roll_deg = msg.vector.y
        # Clamp to reasonable limits for logging
        self.current_pitch_deg = np.clip(self.current_pitch_deg, -90.0, 30.0)
        rospy.loginfo_throttle(0.5, f"Mock Gimbal: Received command Pitch={self.current_pitch_deg:.1f} deg, Roll={self.current_roll_deg:.1f} deg")

    def run(self):
        rospy.loginfo("Mock Gimbal Node Running.")
        while not rospy.is_shutdown():
            pitch_msg = Float64()
            pitch_msg.data = self.current_pitch_deg
            self.pitch_feedback_pub.publish(pitch_msg)
            self.rate.sleep()

if __name__ == '__main__':
    try:
        mock_gimbal = MockGimbal()
        mock_gimbal.run()
    except rospy.ROSInterruptException:
        pass
